import CONSTANT from "../../constant";
import ManifoldPoint from "./manifold-point";
import Method from "../../common/method";

export default class Manifold {
    constructor() {
        this.normal = new Float64Array(3);
        this.tangent = new Float64Array(3);
        this.binormal= new Float64Array(3);
        this._numPoints = 0;
        this._points = new Array(CONSTANT.SETTING_MAX_MANIFOLD_POINTS);
        let _g = 0;
        let _g1 = CONSTANT.SETTING_MAX_MANIFOLD_POINTS;
        while(_g < _g1) this._points[_g++] = new ManifoldPoint();
    }

    _clear() {
        let _g = 0;
        let _g1 = this._numPoints;
        while(_g < _g1) {
            let mp = this._points[_g++];
            let  localPos1=mp.localPos1,localPos2=mp.localPos2,relPos1=mp.relPos1,relPos2=mp.relPos2,pos1=mp.pos1,pos2=mp.pos2;
            localPos1[0] = localPos1[1] = localPos1[2] = 0;
            localPos2[0] = localPos2[1] = localPos2[2] = 0;
            relPos1[0] = relPos1[1] = relPos1[2] = 0;
            relPos2[0] = relPos2[1] = relPos2[2] = 0;
            pos1[0] = pos1[1] = pos1[2] = 0;
            pos2[0] = pos2[1] = pos2[2] = 0;
            mp._depth = 0;
            let imp = mp._impulse;
            imp[0] = imp[1] = imp[2] = imp[3] = imp[4] = imp[5] = imp[6] = 0;
            mp._warmStarted = false;
            mp._disabled = false;
            mp._id = -1;
        }
        this._numPoints = 0;
    }
    _buildBasis(_normal) {
        let tn=this.normal,tt=this.tangent,tb=this.binormal;
        let normal=_normal.elements;
        tn[0] = normal[0];tn[1] = normal[1];tn[2] = normal[2];
        let nx = normal[0], ny = normal[1], nz = normal[2];
        let nx2 = nx * nx, ny2 = ny * ny, nz2 = nz * nz;
        let tx, ty, tz;
        let bx, by, bz;
        if(nx2 < ny2) {
            if(nx2 < nz2) {
                let invL = 1 / Math.sqrt(ny2 + nz2);
                tx = 0;ty = -nz * invL;tz = ny * invL;
                bx = ny * tz - nz * ty;by = -nx * tz;bz = nx * ty;
            } else {
                let invL = 1 / Math.sqrt(nx2 + ny2);
                tx = -ny * invL;ty = nx * invL;tz = 0;
                bx = -nz * ty;by = nz * tx;bz = nx * ty - ny * tx;
            }
        } else if(ny2 < nz2) {
            let invL = 1 / Math.sqrt(nx2 + nz2);
            tx = nz * invL;ty = 0;tz = -nx * invL;
            bx = ny * tz;by = nz * tx - nx * tz;bz = -ny * tx;
        } else {
            let invL = 1 / Math.sqrt(nx2 + ny2);
            tx = -ny * invL;ty = nx * invL;tz = 0;
            bx = -nz * ty;by = nz * tx;bz = nx * ty - ny * tx;
        }
        tt[0] = tx;tt[1] = ty;tt[2] = tz;
        tb[0] = bx;tb[1] = by;tb[2] = bz;
    }
    _updateDepthsAndPositions(_tf1,_tf2) {
        let tf1=_tf1.elements,tf2=_tf2.elements;
        let tn=this.normal;
        let _g = 0, _g1 = this._numPoints;
        while(_g < _g1) {
            let p = this._points[_g++];
            let relPos1=p.relPos1,localPos1=p.localPos1,pos1=p.pos1,relPos2=p.relPos2,localPos2=p.localPos2,pos2=p.pos2;
            relPos1[0] = tf1[3] * localPos1[0] + tf1[4] * localPos1[1] + tf1[5] * localPos1[2];
            relPos1[1] = tf1[6] * localPos1[0] + tf1[7] * localPos1[1] + tf1[8] * localPos1[2];
            relPos1[2] = tf1[9] * localPos1[0] + tf1[10] * localPos1[1] + tf1[11] * localPos1[2];
            relPos2[0] = tf2[3] * localPos2[0] + tf2[4] * localPos2[1] + tf2[5] * localPos2[2];
            relPos2[1] = tf2[6] * localPos2[0] + tf2[7] * localPos2[1] + tf2[8] * localPos2[2];
            relPos2[2] = tf2[9] * localPos2[0] + tf2[10] * localPos2[1] + tf2[11] * localPos2[2];
            pos1[0] = relPos1[0] + tf1[0];pos1[1] = relPos1[1] + tf1[1];pos1[2] = relPos1[2] + tf1[2];
            pos2[0] = relPos2[0] + tf2[0];pos2[1] = relPos2[1] + tf2[1];pos2[2] = relPos2[2] + tf2[2];
            let diffX = pos1[0] - pos2[0],diffY = pos1[1] - pos2[1],diffZ = pos1[2] - pos2[2];
            p._depth = -(diffX * tn[0] + diffY * tn[1] + diffZ * tn[2]);
        }
    }
    getNormal() {
        return Method.copyVec3(this.normal,null);
    }
    getNormalTo(normal) {
        Method.setXYZ(normal,this.normal[0],this.normal[1],this.normal[2]);
    }
    getTangent() {
        return Method.copyVec3(this.tangent,null);
    }
    getTangentTo(tangent) {
        Method.setXYZ(tangent,this.tangent[0],this.tangent[1],this.tangent[2]);
    }
    getBinormal() {
        return Method.copyVec3(this.binormal,null);
    }
    getBinormalTo(binormal) {
        Method.setXYZ(binormal,this.binormal[0],this.binormal[1],this.binormal[2]);
    }
    getPoints() {
        return this._points;
    }
    getNumPoints() {
        return this._numPoints;
    }
}